/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "EnvironmentMeasurement2D.h"

/**
* Default constructor
*/
CEnvironmentMeasurement2D::CEnvironmentMeasurement2D(){
}
/**
* Constructor for net packet
**/
CEnvironmentMeasurement2D::CEnvironmentMeasurement2D(const char *in,unsigned int data){
	setNetPacket(in,data);
}
/**
 * Constructor for Occupance grid
 **/
CEnvironmentMeasurement2D::CEnvironmentMeasurement2D(TOccupancyGrid *g, float th){
	setOccupancyGrid(g,th);
}

/**
 * Constructor for FILE input
 **/
CEnvironmentMeasurement2D::CEnvironmentMeasurement2D(FILE *f){
	fread(f);
}
		

/**
 * Returns a "block" of data which contains the content of the class
 * @param **out pointer to a pointer where the data will be put tu. If NULL, memory is reserved
 * @returns number of bytes
 **/
unsigned int CEnvironmentMeasurement2D::getNetPacket(char **out){
	if(*out==NULL){
		*out=(char *) malloc(ENV_MEASUREMENT_HEADER_SIZE+MEASUREMENT_POINTS_SIZE*points.size());
	}
	memcpy((void *)(*out),(void *) &frame,ENV_MEASUREMENT_HEADER_SIZE);
	for(unsigned int i=0;i<points.size();i++){
		memcpy((void *)(*out+ENV_MEASUREMENT_HEADER_SIZE+i*MEASUREMENT_POINTS_SIZE),(void *) &points[i],MEASUREMENT_POINTS_SIZE);
	}
		
	return ENV_MEASUREMENT_HEADER_SIZE+MEASUREMENT_POINTS_SIZE*points.size();
}

/**
* Creates the class from a block of data.
* @param *in a pointer to data generated by a "getNetPacket" method
* @param size the number of bytes in *in
* @return true if success
**/
bool CEnvironmentMeasurement2D::setNetPacket(const char *in, unsigned int size){
	if(in == NULL || size < ENV_MEASUREMENT_HEADER_SIZE || (size-ENV_MEASUREMENT_HEADER_SIZE)%MEASUREMENT_POINTS_SIZE!=0){
		fprintf(stderr,"CEnvironmentMeasurement2D::setNetPacket:Invalid parameters!\n");
		return false;
	}
	memcpy((void *)&frame,(void *)in,ENV_MEASUREMENT_HEADER_SIZE);
	unsigned int N = (size-ENV_MEASUREMENT_HEADER_SIZE)/MEASUREMENT_POINTS_SIZE;
	TMeasurementPoint p;
	if(points.size()>0) points.clear();
	for(unsigned int i=0;i<N;i++){
		memcpy((void *)&p,(void *)(in+ENV_MEASUREMENT_HEADER_SIZE+i*MEASUREMENT_POINTS_SIZE),MEASUREMENT_POINTS_SIZE);
		points.push_back(p);
	}
	return true;
}

void CEnvironmentMeasurement2D::print(){
	frame.print();
	for(unsigned int i=0;i<points.size();i++){
		fprintf(stderr,"(%.2f,%.2f) ",points[i].x,points[i].y);
	}
	fprintf(stderr,"\n");
}

/**
 * output to file
 */
void CEnvironmentMeasurement2D::fprint(FILE *f){
	if(f==NULL){
		fprintf(stderr,"CEnvironmentMeasurement2D::fprint: Output file ptr is NULL\n");
		return;
	}
	if(points.size()<2){
			fprintf(stderr,"CEnvironmentMeasurement2D::fprint:Invalid number of points!!!\n");
		return;
	} 
	frame.fprint(f);
	for(unsigned int i=0;i<points.size()-1;i++){
		fprintf(f,"%.2f %.2f ",points[i].x,points[i].y);
	}
	fprintf(f,"%.2f %.2f\n",points[points.size()-1].x,points[points.size()-1].y);
	fflush(f);
}
		
/**
 * input from file
 */
void CEnvironmentMeasurement2D::fread(FILE *f){
	if(f==NULL){
		fprintf(stderr,"CEnvironmentMeasurement2D::fprint: Input file ptr is NULL\n");
		return;
	}
	points.clear();
	frame.fread(f);
	TMeasurementPoint p;
	char c=0;
	while( (c!='\n')  ){
		if(feof(f)) break;
		fscanf(f,"%f %f",&p.x,&p.y);
		fscanf(f,"%c",&c);
		points.push_back(p);
	}
}



/**
 * Sets an occupancy grid map to points vector. 
 * The grid cells are intepreted as measurement if the value is grater than th.
 * NOTE: The header (frame) is not set.
 * @param *g pointer to an occupancy grid
 * @param th threshold for choosing measurement points
 */
bool CEnvironmentMeasurement2D::setOccupancyGrid(TOccupancyGrid *g, float th){
	if(g==NULL) return false;
	
	float *ptr = g->getDataPtr();
	int Nx,Ny;
	
	g->getDim(Nx,Ny);
	int size = Nx*Ny;
	points.clear();				
	Grid::TCoord2D coord;
	TMeasurementPoint p;
	//fprintf(stderr,"Setting occupancy grid\n");
	
	for(int i=0;i<size;i++){///<go through the whole grid
		if(ptr[i]>th){
			coord = g->toCoord(Grid::TPixel(i%Nx,i/Nx));
			p.x = coord.x;p.y=coord.y;
			points.push_back(p);
			//fprintf(stderr,"(%.2f,%.2f) ",coord.x,coord.y);
		}
	}		
	return true;
}
		
		

